'''
Created on  6. 14 . 2020
@author: 晨遇韬
@version: V.2.1.0
@Descriptions:解装了各类函数
'''
#from usart import *
from pyb import UART
import sensor, image, time,math
import cpufreq
import ustruct
#green_threshold =(10, 89, -72, -30, -27, 127)
blue_threshold=(16, 72, -24, 31, -93, -30)
green_threshold =(9, 44, -60, -5, 7, 48)
red_threshold =(0, 100, -76, 19, -49, 62)
thresholds = (90, 100, 100, 255, 90, 100)
K=3340    #常数
#传感器初始化
sensor.reset()
sensor.set_pixformat(sensor.RGB565) # grayscale is faster (160x120 max on OpenMV-M7)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_whitebal(True) # turn this off.关闭白平衡
sensor.set_framesize(sensor.QVGA)
sensor.set_auto_gain(False)
#总控制机初始化
cpufreq.set_frequency(216)
uart = UART(3, 9600 )
uart.init(9600,bits=8, timeout_char=20,read_buf_len = 6)
c =[]
while(True):
    #uart.read(10)
    if uart.any() :
        while (uart.readchar() == 0xAA):
            if (uart.readchar() == 0xAE):
                for i in range(4):
                    b = uart.readchar()
                    c.append(b)
                print(c)
            else:
               break
    img = sensor.snapshot().lens_corr(1.8)#畸变矫r正
    img.gaussian(1)#模糊
    photo = img.find_blobs([blue_threshold], pixels_threshold=250, area_threshold=2000, merge=True)
    for blob in photo:
        ratio = blob.w() / blob.h()
        if  ratio <2.9 and ratio>2.3:
            img.draw_edges(blob.min_corners(), color=(255,0,0))
            img.draw_line(blob.major_axis_line(), color=(0,255,0))
            img.draw_line(blob.minor_axis_line(), color=(0,0,255))
            Lm =(blob[2]+blob[3])/2
            length = K/Lm
            print("距离：",length)#单位cm
            print("宽：",blob.w())#单位cm
            print("高：",blob.h())#单位cm
            print("zc：",blob[5])#单位cm
            temp = ustruct.pack("<bbiiiibb",                #格式为2个字符2个整型
                           0xAA,                       #帧头1
                           0xAE,                       #帧头2
                           int(length), # up sample by 4    #数据2
                           int(blob[5]),   # up sample by 4    #数据3 x
                           int(blob[6]), # up sample by 4    #数据4 y
                           int(0xFFFFFFFF), # up sample by 4    #数据1  (保留)
                           0x3D,
                           0xAD)
            uart.write(temp)
    c =[]
